// driver for a differential four-wheel robot
#include "arpa/inet.h"
#include "nlohmann/json.hpp"
#include "sys/socket.h"
#include <string>
typedef struct
{
	int fl_pos;
	int fl_vel;
	int fr_pos;
	int fr_vel;
	int bl_pos;
	int bl_vel;
	int br_pos;
	int br_vel;
} motor_data_t;

class Driver
{
  public:
	double _wheel_base, _track_width, _wheel_diameter;
	double _scale_pos, _scale_vel;
	int sock;
	struct sockaddr_in remote_addr, local_addr;
	bool _loopback;

	Driver(double wheel_base, double track_width, double wheel_diameter, double scale_pos,
		   double scale_vel, std::string remote_ip, std::string local_ip, bool loopback);
	~Driver();
	virtual int driver_move(double linear, double angula);
	virtual int resolve_to_motor(double linear, double angula, motor_data_t &data);
	virtual int hw_write_driver_data(motor_data_t &data);
	virtual int hw_write_driver_vel(double linear, double angula);
	virtual int hw_write_driver_cmd(int cmd);
	virtual int write_json(nlohmann::json jout);
	virtual int vxvy_to_posvel(double vx, double vy, int &pos, int &vel);
};

class OmnidirectionalDriver : public Driver
{
  public:
	OmnidirectionalDriver(double wheel_base, double track_width, double wheel_diameter,
						  double scale_pos, double scale_vel, std::string remote_ip,
						  std::string local_ip, bool loopback)
		: Driver(wheel_base, track_width, wheel_diameter, scale_pos, scale_vel, remote_ip,
				 local_ip, loopback){};
	int resolve_to_motor(double linear, double angula, motor_data_t &data) override;
};

class DifferentialDriver : public Driver
{
  public:
	DifferentialDriver(double wheel_base, double track_width, double wheel_diameter,
					   double scale_pos, double scale_vel, std::string remote_ip,
					   std::string local_ip, bool loopback)
		: Driver(wheel_base, track_width, wheel_diameter, scale_pos, scale_vel, remote_ip,
				 local_ip, loopback){};
	int resolve_to_motor(double linear, double angula, motor_data_t &data) override;
};

class AckermanDriver : public Driver
{
  public:
	AckermanDriver(double wheel_base, double track_width, double wheel_diameter,
				   double scale_pos, double scale_vel, std::string remote_ip,
				   std::string local_ip, bool loopback)
		: Driver(wheel_base, track_width, wheel_diameter, scale_pos, scale_vel, remote_ip,
				 local_ip, loopback){};
	int resolve_to_motor(double linear, double angula, motor_data_t &data) override;
};
